/**
 * @file chassis.c
 * @author NeoZeng neozng1@hnu.edu.cn
 * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
 *        注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
 *
 * @version 0.1
 * @date 2022-12-04
 *
 * @copyright Copyright (c) 2022
 *
 */

 #include "chassis.h"
 #include "robot_def.h"
 #include "dji_motor.h"
 #include "super_cap.h"
 #include "message_center.h"
 #include "referee_task.h"
 
 #include "general_def.h"
 #include "bsp_dwt.h"
 #include "referee_UI.h"
 #include "arm_math.h"
#include "param_config.h"

 /* 根据robot_def.h中的macro自动计算的参数 */
 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
 
 #define ONE_BOARD
 /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
 #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
 #include "can_comm.h"
 #include "ins_task.h"
 static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
 attitude_t *Chassis_IMU_data;
 #endif // CHASSIS_BOARD
 #ifdef ONE_BOARD
 static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
 static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
 #endif                                              // !ONE_BOARD
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
 
 static SuperCapInstance *cap;                                       // 超级电容
 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
 
 /* 用于自旋变速策略的时间变量 */
 // static float t;
 
 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 static PIDInstance Follow_PID;
 
 void ChassisInit()
 {
     RobotParams_t* pid_params = GetRobotPIDParams();
     // 四个轮子的参数一样,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
         .can_init_config.can_handle = &hcan1,
         .controller_param_init_config = {
             .speed_PID = pid_params->chassis_params.speed_PID,
             .current_PID =pid_params->chassis_params.current_PID,
         },
         .controller_setting_init_config = {
             .angle_feedback_source = MOTOR_FEED,
             .speed_feedback_source = MOTOR_FEED,
             .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP,
         },
         .motor_type = pid_params->chassis_params.motor_type,
     };
 
   PID_Init_Config_s follow_pid_config=pid_params->chassis_params.follow_PID;
      PIDInit(&Follow_PID,&follow_pid_config);
     //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
     chassis_motor_config.can_init_config.tx_id = 1;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_lf = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 4;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_rf = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 2;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_lb = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 3;
     chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
     motor_rb = DJIMotorInit(&chassis_motor_config);
 
     //referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
 
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
             .can_handle = &hcan2,
             .tx_id = 0x302, // 超级电容默认接收id
             .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
         }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
 
     CANComm_Init_Config_s comm_conf = {
         .can_config = {
             .can_handle = &hcan2,
             .tx_id = 0x311,
             .rx_id = 0x312,
         },
         .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
         .send_data_len = sizeof(Chassis_Upload_Data_s),
     };
     chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
 #endif                                          // CHASSIS_BOARD
 
 #ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
     chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
     chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
 #endif // ONE_BOARD
 }
 
 #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 /**
  * @brief 计算每个轮毂电机的输出,正运动学解算
  *        用宏进行预替换减小开销,运动解算具体过程参考教程
  */
 static void MecanumCalculate()
 {
     vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
     vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
     vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
     vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
     DJMotorPIDCal(motor_lf, vt_lf);
     DJMotorPIDCal(motor_rf, vt_rf);
     DJMotorPIDCal(motor_lb, vt_lb);
     DJMotorPIDCal(motor_rb, vt_rb);
 }
 
 
 
 /**
  * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
  *
  */
 static void PowerControl(){
           // 功率模型常量
   static const float k0 = 0.7441993412640775f;
   static const float k1 = 0.006444284468539646f;
   static const float k2 = 0.0001423857226262331f;
   static const float k3 = 0.015644430204543864f;
   static const float k4 = 0.1580143850678086f;
   static const float k5 = 2.896721772539512e-05f;
   // 获取电机速度反馈,化成单位rad/s
   float motor_speed_fdb[4] = {
     (float)motor_lf->measure.speed_aps/6.f,
     (float)motor_rf->measure.speed_aps/6.f,
     (float)motor_lb->measure.speed_aps/6.f,
     (float)motor_rb->measure.speed_aps/6.f,
 };
   // 获取当前电机参考电流，统一位单位为A
   float motor_current_list[4] = {
     (float)motor_lf->motor_controller.final_output,
     (float)motor_rf->motor_controller.final_output,
     (float)motor_lb->motor_controller.final_output,
     (float)motor_rb->motor_controller.final_output,
 };

   float initial_give_power[4] = {0.0f};//每个电机的初始估计功率
   float initial_total_power = 0.0f;//估计初始总功率

   // 计算每个电机的功率贡献
   for (int i = 0; i < 4; i++) {    initial_give_power[i] =
         k0 + k1 * motor_current_list[i]/ (16384.0f / 20.0f) + k2 * motor_speed_fdb[i] * (2.0f * PI / 60.0f) +
         k3 * motor_current_list[i] / (16384.0f / 20.0f) * motor_speed_fdb[i]* (2.0f * PI / 60.0f) +
         k4 * motor_current_list[i]/ (16384.0f / 20.0f) * motor_current_list[i]/ (16384.0f / 20.0f) +
         k5 * motor_speed_fdb[i] * (2.0f * PI / 60.0f) * motor_speed_fdb[i]* (2.0f * PI / 60.0f);

     // 只累加正向功率
     if (initial_give_power[i] > 0) {
       initial_total_power += initial_give_power[i];
     }
   }
   // 功率超限时进行动态调整
   if (initial_total_power > (float)chassis_cmd_recv.max_power ) {
     float power_scale = (float)chassis_cmd_recv.max_power / initial_total_power;//削减功率比例
     float scaled_give_power[4];
     // 计算缩放后的功率目标
     for (int i = 0; i < 4; i++) {
       scaled_give_power[i] = initial_give_power[i] * power_scale;
     }

     // 重新计算每个电机的电流参考值
     for (int i = 0; i < 4; i++) {
       // 二次方程系数计算，参数
       float a = k4 / (16384.0f / 20.0f) / (16384.0f / 20.0f);
       float b =
           k1 / (16384.0f / 20.0f) + k3 * motor_speed_fdb[i] * (2.0f * PI / 60.0f) / (16384.0f / 20.0f);
       float c =
           k2 * motor_speed_fdb[i] * (2.0f * PI / 60.0f) +
           k5 * motor_speed_fdb[i] * (2.0f * PI / 60.0f) * motor_speed_fdb[i] * (2.0f * PI / 60.0f) -
           scaled_give_power[i] + k0;
       float discriminant = b * b - 4 * a * c;  // 判别式
       if (discriminant >= 0) {
         float sqrt_disc = sqrtf(discriminant);
         float temp1 = (-b + sqrt_disc) / (2 * a);
         float temp2 = (-b - sqrt_disc) / (2 * a);

         // 选择最接近当前电流的解
         if (motor_current_list[i] > 0) {
           motor_current_list[i] = (fabsf(temp1 - motor_current_list[i]) < fabsf(temp2 - motor_current_list[i])) ?
                                   fminf(16000.f, temp1) : fminf(16000.f, temp2);
         } else {
           motor_current_list[i] = (fabsf(temp1 - motor_current_list[i]) < fabsf(temp2 - motor_current_list[i])) ?
                                   fmaxf(-16000.f, temp1) : fmaxf(-16000.f, temp2);
         }
       } else {
         // 无解时归零
         motor_current_list[i] = 0.0f;
       }
     }
    }
   motor_lf->motor_controller.final_output=(int16_t)(motor_current_list[0]);// 更新全局参考电流值
   motor_rf->motor_controller.final_output=(int16_t)(motor_current_list[1]);// 更新全局参考电流值
   motor_lb->motor_controller.final_output=(int16_t)(motor_current_list[2]);// 更新全局参考电流值
   motor_rb->motor_controller.final_output=(int16_t)(motor_current_list[3]);// 更新全局参考电流值

 }
 
 static void LimitChassisOutput()
 {
     // 功率限制待添加
     // referee_data->PowerHeatData.chassis_power;
     // referee_data->PowerHeatData.chassis_power_buffer;
 
     // 完成功率限制后进行电机参考输入设定
 }
 
 /**
  * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
  *        对于双板的情况,考虑增加来自底盘板IMU的数据
  *
  */
 static void EstimateSpeed()
 {
     // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
     // chassis_feedback_data.vx vy wz =
     //  ...
 }
 
 /* 机器人底盘控制核心任务 */
 void ChassisTask()
 {
     // 后续增加没收到消息的处理(双板的情况)
     // 获取新的控制信息
 #ifdef ONE_BOARD
     SubGetMessage(chassis_sub, &chassis_cmd_recv);
 #endif
 #ifdef CHASSIS_BOARD
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
 
     if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
     { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         DJIMotorStop(motor_lf);
         DJIMotorStop(motor_rf);
         DJIMotorStop(motor_lb);
         DJIMotorStop(motor_rb);
     }
     else
     { // 正常工作
         DJIMotorEnable(motor_lf);
         DJIMotorEnable(motor_rf);
         DJIMotorEnable(motor_lb);
         DJIMotorEnable(motor_rb);
     }
 
     // 根据控制模式设定旋转速度
     switch (chassis_cmd_recv.chassis_mode)
     {
     case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
         //chassis_cmd_recv.wz = 0;
         break;
     case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
        // chassis_cmd_recv.wz = -2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
        chassis_cmd_recv.wz+=PIDCalculate(&Follow_PID,chassis_cmd_recv.offset_angle,0);
         break;
     case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
         // chassis_cmd_recv.wz = 4000;
         break;
     default:
         break;
     }
 
     // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
     // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
     static float sin_theta, cos_theta;
     cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
     sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
     chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 
     // 根据控制模式进行正运动学解算,计算底盘输出
     MecanumCalculate();
 
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
       PowerControl();
 
 
     LimitChassisOutput();
 
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
 
     // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
     // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
     // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
     // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
     // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
     // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
     // 推送反馈消息
 #ifdef ONE_BOARD
     PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
 #endif
 #ifdef CHASSIS_BOARD
     CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
 #endif // CHASSIS_BOARD
 }